using System;
using System.Diagnostics;
using Microsoft.Xna.Framework;

namespace Oops.Xna.Framework
{
	/// <summary>
	/// Represents an axis of a <see cref="Microsoft.Xna.Framework.Matrix" />.
	/// </summary>
	public enum MatrixAxis
	{
		/// <summary>
		/// The X-axis of a <see cref="Microsoft.Xna.Framework.Matrix" />.
		/// </summary>
		X,

		/// <summary>
		/// The X-axis of a <see cref="Microsoft.Xna.Framework.Matrix" />.
		/// </summary>
		Y,

		/// <summary>
		/// The X-axis of a <see cref="Microsoft.Xna.Framework.Matrix" />.
		/// </summary>
		Z,

		/// <summary>
		/// The translation axis of a <see cref="Microsoft.Xna.Framework.Matrix" />.
		/// </summary>
		Translation
	}

	/// <summary>
	/// An extension to the <see cref="Microsoft.Xna.Framework.Matrix" /> structure. 
	/// </summary>
	public static class MatrixExtension
	{
		/// <summary>
		/// Returns a <see cref="Matrix"/> with all it's elements set to zero. 
		/// </summary>
		public static readonly Matrix Zero = new Matrix();

		/// <summary>
		/// Gets the chosen axis from the specified <see cref="Microsoft.Xna.Framework.Matrix"/>.
		/// </summary>
		/// <param name="matrix">The source <see cref="Microsoft.Xna.Framework.Matrix"/>.</param>
		/// <param name="axis">The rotational axis to extract.</param>
		/// <returns>Returns a <see cref="Microsoft.Xna.Framework.Vector3"/> representing a rotational axis.</returns>
		public unsafe static Vector3 GetAxis(this Matrix matrix, MatrixAxis axis)
		{
#if XBOX
			Vector3 result = Vector3.Zero;
#else
			Vector3 result;
#endif
			int index = (int)axis << 2;
			Matrix* address = &matrix;
			result.X = ((float*)address)[index];
			result.Y = ((float*)address)[index + 1];
			result.Z = ((float*)address)[index + 2];
			
			return result;
		}

		/// <summary>
		/// Gets the chosen axis from the specified <see cref="Microsoft.Xna.Framework.Matrix"/>.
		/// </summary>
		/// <param name="matrix">The source <see cref="Microsoft.Xna.Framework.Matrix"/>.</param>
		/// <param name="axis">The rotational axis to extract.</param>
		/// <param name="result">A <see cref="Microsoft.Xna.Framework.Vector3"/> representing a rotational axis.</param>
		public unsafe static void GetAxis(ref Matrix matrix, MatrixAxis axis, out Vector3 result)
		{
#if XBOX
			result = Vector3.Zero;
#endif
			int index = (int)axis << 2;
			fixed (Matrix* address = &matrix)
			{
				result.X = ((float*)address)[index];
				result.Y = ((float*)address)[index + 1];
				result.Z = ((float*)address)[index + 2];
			}
		}
		
		/// <summary>
		/// Sets the chosen axes of a <see cref="Microsoft.Xna.Framework.Matrix"/> with the specified <see cref="Microsoft.Xna.Framework.Vector3"/>.
		/// </summary>
		/// <param name="matrix">The source <see cref="Microsoft.Xna.Framework.Matrix"/>.</param>
		/// <param name="axis">The axis to modify.</param>
		/// <param name="value">A <see cref="Microsoft.Xna.Framework.Vector3"/> representing the new axis.</param>
		public unsafe static void SetAxis(ref Matrix matrix, MatrixAxis axis, Vector3 value)
		{
			int index = (int)axis << 2;
			fixed (Matrix* address = &matrix)
			{
				((float*)address)[index] = value.X;
				((float*)address)[index + 1] = value.Y;
				((float*)address)[index + 2] = value.Z;
			}
		}

		/// <summary>
		/// Multiplies the two <see cref="Microsoft.Xna.Framework.Matrix"/> instances together, ignoring the 4th row and column.
		/// </summary>
		/// <param name="matrix1">Source matrix.</param>
		/// <param name="matrix2">Source matrix.</param>
		/// <returns>Result of the multiplication.</returns>
		public static Matrix Multiply33(Matrix matrix1, Matrix matrix2)
		{
			Matrix result = MatrixExtension.Zero;

			result.M11 = (matrix1.M11 * matrix2.M11) + (matrix1.M12 * matrix2.M21) + (matrix1.M13 * matrix2.M31);
			result.M12 = (matrix1.M11 * matrix2.M12) + (matrix1.M12 * matrix2.M22) + (matrix1.M13 * matrix2.M32);
			result.M13 = (matrix1.M11 * matrix2.M13) + (matrix1.M12 * matrix2.M23) + (matrix1.M13 * matrix2.M33);
			result.M14 = 0.0f;

			result.M21 = (matrix1.M21 * matrix2.M11) + (matrix1.M22 * matrix2.M21) + (matrix1.M23 * matrix2.M31);
			result.M22 = (matrix1.M21 * matrix2.M12) + (matrix1.M22 * matrix2.M22) + (matrix1.M23 * matrix2.M32);
			result.M23 = (matrix1.M21 * matrix2.M13) + (matrix1.M22 * matrix2.M23) + (matrix1.M23 * matrix2.M33);
			result.M24 = 0.0f;

			result.M31 = (matrix1.M31 * matrix2.M11) + (matrix1.M32 * matrix2.M21) + (matrix1.M33 * matrix2.M31);
			result.M32 = (matrix1.M31 * matrix2.M12) + (matrix1.M32 * matrix2.M22) + (matrix1.M33 * matrix2.M32);
			result.M33 = (matrix1.M31 * matrix2.M13) + (matrix1.M32 * matrix2.M23) + (matrix1.M33 * matrix2.M33);
			result.M34 = 0.0f;

			result.M41 = 0.0f;
			result.M42 = 0.0f;
			result.M43 = 0.0f;
			result.M44 = 0.1f;

			return result;
		}

		/// <summary>
		/// Multiplies the two <see cref="Microsoft.Xna.Framework.Matrix"/> instances together, ignoring the 4th row and column.
		/// </summary>
		/// <param name="matrix1">Source <see cref="Microsoft.Xna.Framework.Matrix"/>.</param>
		/// <param name="matrix2">Source <see cref="Microsoft.Xna.Framework.Matrix"/>.</param>
		/// <param name="result">Result of the multiplication.</param>
		public static void Multiply33(ref Matrix matrix1, ref Matrix matrix2, out Matrix result)
		{
			float m11 = (matrix1.M11 * matrix2.M11) + (matrix1.M12 * matrix2.M21) + (matrix1.M13 * matrix2.M31);
			float m12 = (matrix1.M11 * matrix2.M12) + (matrix1.M12 * matrix2.M22) + (matrix1.M13 * matrix2.M32);
			float m13 = (matrix1.M11 * matrix2.M13) + (matrix1.M12 * matrix2.M23) + (matrix1.M13 * matrix2.M33);
			float m21 = (matrix1.M21 * matrix2.M11) + (matrix1.M22 * matrix2.M21) + (matrix1.M23 * matrix2.M31);
			float m22 = (matrix1.M21 * matrix2.M12) + (matrix1.M22 * matrix2.M22) + (matrix1.M23 * matrix2.M32);
			float m23 = (matrix1.M21 * matrix2.M13) + (matrix1.M22 * matrix2.M23) + (matrix1.M23 * matrix2.M33);
			float m31 = (matrix1.M31 * matrix2.M11) + (matrix1.M32 * matrix2.M21) + (matrix1.M33 * matrix2.M31);
			float m32 = (matrix1.M31 * matrix2.M12) + (matrix1.M32 * matrix2.M22) + (matrix1.M33 * matrix2.M32);
			float m33 = (matrix1.M31 * matrix2.M13) + (matrix1.M32 * matrix2.M23) + (matrix1.M33 * matrix2.M33);

#if XBOX
			result = MatrixExtension.Zero;
#endif
			result.M11 = m11;
			result.M12 = m12;
			result.M13 = m13;
			result.M14 = 0.0f;
			result.M21 = m21;
			result.M22 = m22;
			result.M23 = m23;
			result.M24 = 0.0f;
			result.M31 = m31;
			result.M32 = m32;
			result.M33 = m33;
			result.M34 = 0.0f;
			result.M41 = 0.0f;
			result.M42 = 0.0f;
			result.M43 = 0.0f;
			result.M44 = 1.0f;
		}

		/// <summary>
		/// Multiplies a <see cref="Microsoft.Xna.Framework.Matrix"/> by the transpose of another <see cref="Microsoft.Xna.Framework.Matrix"/>, ignoring the 4th row and column.
		/// </summary>
		/// <param name="matrix1">Source <see cref="Microsoft.Xna.Framework.Matrix"/>.</param>
		/// <param name="matrix2">Source <see cref="Microsoft.Xna.Framework.Matrix"/> that will be transposed.</param>
		/// <returns>Result of the multiplication.</returns>
		public static Matrix MultiplyTranspose33(Matrix matrix1, Matrix matrix2)
		{
#if XBOX
			Matrix result = MatrixExtension.Zero;
#else
			Matrix result;
#endif
			result.M11 = matrix1.M11 * matrix2.M11 + matrix1.M12 * matrix2.M12 + matrix1.M13 * matrix2.M13;
			result.M12 = matrix1.M11 * matrix2.M21 + matrix1.M12 * matrix2.M22 + matrix1.M13 * matrix2.M23;
			result.M13 = matrix1.M11 * matrix2.M31 + matrix1.M12 * matrix2.M32 + matrix1.M13 * matrix2.M33;
			result.M14 = 0.0f;
			result.M21 = matrix1.M21 * matrix2.M11 + matrix1.M22 * matrix2.M12 + matrix1.M23 * matrix2.M13;
			result.M22 = matrix1.M21 * matrix2.M21 + matrix1.M22 * matrix2.M22 + matrix1.M23 * matrix2.M23;
			result.M23 = matrix1.M21 * matrix2.M31 + matrix1.M22 * matrix2.M32 + matrix1.M23 * matrix2.M33;
			result.M24 = 0.0f;
			result.M31 = matrix1.M31 * matrix2.M11 + matrix1.M32 * matrix2.M12 + matrix1.M33 * matrix2.M13;
			result.M32 = matrix1.M31 * matrix2.M21 + matrix1.M32 * matrix2.M22 + matrix1.M33 * matrix2.M23;
			result.M33 = matrix1.M31 * matrix2.M31 + matrix1.M32 * matrix2.M32 + matrix1.M33 * matrix2.M33;
			result.M34 = 0.0f;
			result.M41 = 0.0f;
			result.M42 = 0.0f;
			result.M43 = 0.0f;
			result.M44 = 1.0f;
			

			return result;
		}

		/// <summary>
		/// Multiplies a <see cref="Microsoft.Xna.Framework.Matrix"/> by the transpose of another <see cref="Microsoft.Xna.Framework.Matrix"/>, ignoring the 4th row and column.
		/// </summary>
		/// <param name="matrix1">Source <see cref="Microsoft.Xna.Framework.Matrix"/>.</param>
		/// <param name="matrix2">Source <see cref="Microsoft.Xna.Framework.Matrix"/> that will be transposed.</param>
		/// <param name="result">Result of the multiplication.</param>
		public static void MultiplyTranspose33(ref Matrix matrix1, ref Matrix matrix2, out Matrix result)
		{
#if XBOX
			result = MatrixExtension.Zero;
#endif
			result.M11 = matrix1.M11 * matrix2.M11 + matrix1.M12 * matrix2.M12 + matrix1.M13 * matrix2.M13;
			result.M12 = matrix1.M11 * matrix2.M21 + matrix1.M12 * matrix2.M22 + matrix1.M13 * matrix2.M23;
			result.M13 = matrix1.M11 * matrix2.M31 + matrix1.M12 * matrix2.M32 + matrix1.M13 * matrix2.M33;
			result.M14 = 0.0f;
			result.M21 = matrix1.M21 * matrix2.M11 + matrix1.M22 * matrix2.M12 + matrix1.M23 * matrix2.M13;
			result.M22 = matrix1.M21 * matrix2.M21 + matrix1.M22 * matrix2.M22 + matrix1.M23 * matrix2.M23;
			result.M23 = matrix1.M21 * matrix2.M31 + matrix1.M22 * matrix2.M32 + matrix1.M23 * matrix2.M33;
			result.M24 = 0.0f;
			result.M31 = matrix1.M31 * matrix2.M11 + matrix1.M32 * matrix2.M12 + matrix1.M33 * matrix2.M13;
			result.M32 = matrix1.M31 * matrix2.M21 + matrix1.M32 * matrix2.M22 + matrix1.M33 * matrix2.M23;
			result.M33 = matrix1.M31 * matrix2.M31 + matrix1.M32 * matrix2.M32 + matrix1.M33 * matrix2.M33;
			result.M34 = 0.0f;
			result.M41 = 0.0f;
			result.M42 = 0.0f;
			result.M43 = 0.0f;
			result.M44 = 1.0f;
		}

		/// <summary>
		/// Calculates the inverse of a <see cref="Matrix.Xna.Framework.Matrix"/>, ignoring the 4th row and column.
		/// </summary>
		/// <param name="matrix">Source <see cref="Microsoft.Xna.Framework.Matrix"/>.</param>
		/// <returns>The inverse of the <paramref name="matrix"/>.</returns>
		public static Matrix Invert33(Matrix matrix)
		{
			// Calculate reusable values.
			float m11m22 = matrix.M11 * matrix.M22;
			float m11m32 = matrix.M11 * matrix.M32;
			float m21m12 = matrix.M21 * matrix.M12;
			float m31m12 = matrix.M31 * matrix.M12;
			float m21m13 = matrix.M21 * matrix.M13;
			float m31m13 = matrix.M31 * matrix.M13;

			// Calculate the determinant.
			float determinant = 
				m11m22 * matrix.M33 - 
				m11m32 * matrix.M23 - 
				m21m12 * matrix.M33 +
				m31m12 * matrix.M23 + 
				m21m13 * matrix.M32 - 
				m31m13 * matrix.M22;

			// Assert determinant value.
			Debug.Assert(determinant != 0.0f);

			// Get the determinant reciprocal.
			float inverse = 1.0f / determinant;

			// Create a 3 x 3 inverse matrix from the specified.  Use identity for 4th-dimension values.
			Matrix result = MatrixExtension.Zero;
			result.M11 = (matrix.M22 * matrix.M33 - matrix.M32 * matrix.M23) * inverse;
			result.M12 = -(matrix.M12 * matrix.M33 - matrix.M32 * matrix.M13) * inverse;
			result.M13 = (matrix.M12 * matrix.M23 - matrix.M22 * matrix.M13) * inverse;
			result.M14 = 0.0f;

			result.M21 = -(matrix.M21 * matrix.M33 - matrix.M31 * matrix.M23) * inverse;
			result.M22 = (matrix.M11 * matrix.M33 - m31m13) * inverse;
			result.M23 = -(matrix.M11 * matrix.M23 - m21m13) * inverse;
			result.M24 = 0.0f;

			result.M31 = (matrix.M21 * matrix.M32 - matrix.M31 * matrix.M22) * inverse;
			result.M32 = -(m11m32 - m31m12) * inverse;
			result.M33 = (m11m22 - m21m12) * inverse;
			result.M34 = 0.0f;

			result.M41 = 0.0f;
			result.M42 = 0.0f;
			result.M43 = 0.0f;
			result.M44 = 1.0f;

			// Return the result.
			return result;
		}

		/// <summary>
		/// Calculates the inverse of a <see cref="Matrix.Xna.Framework.Matrix"/>, ignoring the 4th row and column.
		/// </summary>
		/// <param name="matrix">Source <see cref="Microsoft.Xna.Framework.Matrix"/>.</param>
		/// <param name="result">The inverse of the <paramref name="matrix"/>.</param>
		public static void Invert33(ref Matrix matrix, out Matrix result)
		{
			// Calculate reusable values.
			float m11m22 = matrix.M11 * matrix.M22;
			float m11m32 = matrix.M11 * matrix.M32;
			float m21m12 = matrix.M21 * matrix.M12;
			float m31m12 = matrix.M31 * matrix.M12;
			float m21m13 = matrix.M21 * matrix.M13;
			float m31m13 = matrix.M31 * matrix.M13;

			// Calculate the determinant.
			float determinant =
				m11m22 * matrix.M33 -
				m11m32 * matrix.M23 -
				m21m12 * matrix.M33 +
				m31m12 * matrix.M23 +
				m21m13 * matrix.M32 -
				m31m13 * matrix.M22;

			// Assert determinant value.
			Debug.Assert(determinant != 0.0f);

			// Get the determinant reciprocal.
			float inverse = 1.0f / determinant;

			// Create a 3 x 3 inverse matrix from the specified.  Use identity for 4th-dimension values.
#if XBOX
			result = MatrixExtension.Zero;
#endif
			result.M11 = (matrix.M22 * matrix.M33 - matrix.M32 * matrix.M23) * inverse;
			result.M12 = -(matrix.M12 * matrix.M33 - matrix.M32 * matrix.M13) * inverse;
			result.M13 = (matrix.M12 * matrix.M23 - matrix.M22 * matrix.M13) * inverse;
			result.M14 = 0.0f;

			result.M21 = -(matrix.M21 * matrix.M33 - matrix.M31 * matrix.M23) * inverse;
			result.M22 = (matrix.M11 * matrix.M33 - m31m13) * inverse;
			result.M23 = -(matrix.M11 * matrix.M23 - m21m13) * inverse;
			result.M24 = 0.0f;

			result.M31 = (matrix.M21 * matrix.M32 - matrix.M31 * matrix.M22) * inverse;
			result.M32 = -(m11m32 - m31m12) * inverse;
			result.M33 = (m11m22 - m21m12) * inverse;
			result.M34 = 0.0f;

			result.M41 = 0.0f;
			result.M42 = 0.0f;
			result.M43 = 0.0f;
			result.M44 = 1.0f;
		}

		/// <summary>
		/// Transposes the rows and columns of a <see cref="Microsoft.Xna.Framework.Matrix"/>, ignoring the 4th row and column.
		/// </summary>
		/// <param name="matrix">Source <see cref="Microsoft.Xna.Framework.Matrix"/>.</param>
		/// <returns>Transpose of the <paramref name="matrix"/>.</returns>
		public static Matrix Transpose33(Matrix matrix)
		{
			Matrix result = MatrixExtension.Zero;

			result.M11 = matrix.M11;
			result.M12 = matrix.M21;
			result.M13 = matrix.M31;
			result.M14 = 0.0f;

			result.M21 = matrix.M12;
			result.M22 = matrix.M22;
			result.M23 = matrix.M32;
			result.M24 = 0.0f;

			result.M31 = matrix.M13;
			result.M32 = matrix.M23;
			result.M33 = matrix.M33;
			result.M34 = 0.0f;

			result.M41 = 0.0f;
			result.M42 = 0.0f;
			result.M43 = 0.0f;
			result.M44 = 1.0f;

			return result;
		}

		/// <summary>
		/// Transposes the rows and columns of a <see cref="Microsoft.Xna.Framework.Matrix"/>, ignoring the 4th row and column.
		/// </summary>
		/// <param name="matrix">Source <see cref="Microsoft.Xna.Framework.Matrix"/>.</param>
		/// <param name="result">Transpose of the <paramref name="matrix"/>.</param>
		public static void Transpose33(ref Matrix matrix, out Matrix result)
		{
			float m11 = matrix.M11;
			float m12 = matrix.M12;
			float m13 = matrix.M13;
			
			float m21 = matrix.M21;
			float m22 = matrix.M22;
			float m23 = matrix.M23;
			
			float m31 = matrix.M31;
			float m32 = matrix.M32;
			float m33 = matrix.M33;

#if XBOX
			result = MatrixExtension.Zero;
#endif
			result.M11 = m11;
			result.M12 = m21;
			result.M13 = m31;
			result.M14 = 0.0f;

			result.M21 = m12;
			result.M22 = m22;
			result.M23 = m32;
			result.M24 = 0.0f;

			result.M31 = m13;
			result.M32 = m23;
			result.M33 = m33;
			result.M34 = 0.0f;

			result.M41 = 0.0f;
			result.M42 = 0.0f;
			result.M43 = 0.0f;
			result.M44 = 1.0f;
		}

		/// <summary>
		/// 
		/// </summary>
		/// <param name="matrix1"></param>
		/// <param name="matrix2"></param>
		/// <returns></returns>
		public static Matrix ConvertBasis33(Matrix matrix1, Matrix matrix2)
		{
			float m11 = matrix2.M11 * matrix1.M11 + matrix2.M21 * matrix1.M12 + matrix2.M31 * matrix1.M13;
			float m12 = matrix2.M11 * matrix1.M21 + matrix2.M21 * matrix1.M22 + matrix2.M31 * matrix1.M23;
			float m13 = matrix2.M11 * matrix1.M31 + matrix2.M21 * matrix1.M32 + matrix2.M31 * matrix1.M33;

			float m21 = matrix2.M12 * matrix1.M11 + matrix2.M22 * matrix1.M12 + matrix2.M32 * matrix1.M13;
			float m22 = matrix2.M12 * matrix1.M21 + matrix2.M22 * matrix1.M22 + matrix2.M32 * matrix1.M23;
			float m23 = matrix2.M12 * matrix1.M31 + matrix2.M22 * matrix1.M32 + matrix2.M32 * matrix1.M33;

			float m31 = matrix2.M13 * matrix1.M11 + matrix2.M23 * matrix1.M12 + matrix2.M33 * matrix1.M13;
			float m32 = matrix2.M13 * matrix1.M21 + matrix2.M23 * matrix1.M22 + matrix2.M33 * matrix1.M23;
			float m33 = matrix2.M13 * matrix1.M31 + matrix2.M23 * matrix1.M32 + matrix2.M33 * matrix1.M33;

			Matrix result = MatrixExtension.Zero;

			result.M11 = m11 * matrix2.M11 + m12 * matrix2.M21 + m13 * matrix2.M31;
			result.M21 = m11 * matrix2.M12 + m12 * matrix2.M22 + m13 * matrix2.M32;
			result.M31 = m11 * matrix2.M13 + m12 * matrix2.M23 + m13 * matrix2.M33;
			result.M41 = 0.0f;

			result.M12 = m21 * matrix2.M11 + m22 * matrix2.M21 + m23 * matrix2.M31;
			result.M22 = m21 * matrix2.M12 + m22 * matrix2.M22 + m23 * matrix2.M32;
			result.M32 = m21 * matrix2.M13 + m22 * matrix2.M23 + m23 * matrix2.M33;
			result.M42 = 0.0f;

			result.M13 = m31 * matrix2.M11 + m32 * matrix2.M21 + m33 * matrix2.M31;
			result.M23 = m31 * matrix2.M12 + m32 * matrix2.M22 + m33 * matrix2.M32;
			result.M33 = m31 * matrix2.M13 + m32 * matrix2.M23 + m33 * matrix2.M33;
			result.M43 = 0.0f;

			result.M14 = 0.0f;
			result.M24 = 0.0f;
			result.M34 = 0.0f;
			result.M44 = 1.0f;

			return result;
		}

		/// <summary>
		/// 
		/// </summary>
		/// <param name="matrix1"></param>
		/// <param name="matrix2"></param>
		/// <param name="result"></param>
		public static void ConvertBasis33(ref Matrix matrix1, ref Matrix matrix2, out Matrix result)
		{
			float m11 = matrix2.M11 * matrix1.M11 + matrix2.M21 * matrix1.M12 + matrix2.M31 * matrix1.M13;
			float m12 = matrix2.M11 * matrix1.M21 + matrix2.M21 * matrix1.M22 + matrix2.M31 * matrix1.M23;
			float m13 = matrix2.M11 * matrix1.M31 + matrix2.M21 * matrix1.M32 + matrix2.M31 * matrix1.M33;

			float m21 = matrix2.M12 * matrix1.M11 + matrix2.M22 * matrix1.M12 + matrix2.M32 * matrix1.M13;
			float m22 = matrix2.M12 * matrix1.M21 + matrix2.M22 * matrix1.M22 + matrix2.M32 * matrix1.M23;
			float m23 = matrix2.M12 * matrix1.M31 + matrix2.M22 * matrix1.M32 + matrix2.M32 * matrix1.M33;

			float m31 = matrix2.M13 * matrix1.M11 + matrix2.M23 * matrix1.M12 + matrix2.M33 * matrix1.M13;
			float m32 = matrix2.M13 * matrix1.M21 + matrix2.M23 * matrix1.M22 + matrix2.M33 * matrix1.M23;
			float m33 = matrix2.M13 * matrix1.M31 + matrix2.M23 * matrix1.M32 + matrix2.M33 * matrix1.M33;
#if XBOX
			result = MatrixExtension.Zero;
#endif
			result.M11 = m11 * matrix2.M11 + m12 * matrix2.M21 + m13 * matrix2.M31;
			result.M21 = m11 * matrix2.M12 + m12 * matrix2.M22 + m13 * matrix2.M32;
			result.M31 = m11 * matrix2.M13 + m12 * matrix2.M23 + m13 * matrix2.M33;
			result.M41 = 0.0f;

			result.M12 = m21 * matrix2.M11 + m22 * matrix2.M21 + m23 * matrix2.M31;
			result.M22 = m21 * matrix2.M12 + m22 * matrix2.M22 + m23 * matrix2.M32;
			result.M32 = m21 * matrix2.M13 + m22 * matrix2.M23 + m23 * matrix2.M33;
			result.M42 = 0.0f;

			result.M13 = m31 * matrix2.M11 + m32 * matrix2.M21 + m33 * matrix2.M31;
			result.M23 = m31 * matrix2.M12 + m32 * matrix2.M22 + m33 * matrix2.M32;
			result.M33 = m31 * matrix2.M13 + m32 * matrix2.M23 + m33 * matrix2.M33;
			result.M43 = 0.0f;

			result.M14 = 0.0f;
			result.M24 = 0.0f;
			result.M34 = 0.0f;
			result.M44 = 1.0f;
		}

		/// <summary>
		/// 
		/// </summary>
		/// <param name="matrix"></param>
		/// <param name="offset"></param>
		/// <param name="mass"></param>
		/// <returns></returns>
		public static Matrix TransferAxis(Matrix matrix, Vector3 offset, float mass)
		{
			// Hold onto calculations used more than once.
			float m12 = matrix.M12 - mass * offset.X * offset.Y;
			float m23 = matrix.M23 - mass * offset.Y * offset.Z;
			float m31 = matrix.M31 - mass * offset.Z * offset.X;

			Matrix result = MatrixExtension.Zero;

			// Apply the transfer axis theorem.
			result.M11 = matrix.M11 + mass * (offset.Y * offset.Y + offset.Z * offset.Z);
			result.M12 = m12;
			result.M13 = m31;
			result.M14 = 0.0f;

			result.M21 = m12;
			result.M22 = matrix.M22 + mass * (offset.Z * offset.Z + offset.X * offset.X);
			result.M23 = m23;
			result.M24 = 0.0f;

			result.M31 = m31;
			result.M32 = m23;
			result.M33 = matrix.M33 + mass * (offset.X * offset.X + offset.Y * offset.Y);
			result.M34 = 0.0f;

			result.M41 = 0.0f;
			result.M42 = 0.0f;
			result.M43 = 0.0f;
			result.M44 = 1.0f;

			return result;
		}

		/// <summary>
		/// 
		/// </summary>
		/// <param name="matrix"></param>
		/// <param name="offset"></param>
		/// <param name="mass"></param>
		/// <param name="result"></param>
		public static void TransferAxis(ref Matrix matrix, Vector3 offset, float mass, out Matrix result)
		{
			// Hold onto calculations used more than once.
			float m12 = matrix.M12 - mass * offset.X * offset.Y;
			float m23 = matrix.M23 - mass * offset.Y * offset.Z;
			float m31 = matrix.M31 - mass * offset.Z * offset.X;
#if XBOX
			result = MatrixExtension.Zero;
#endif
			// Apply the transfer axis theorem.
			result.M11 = matrix.M11 + mass * (offset.Y * offset.Y + offset.Z * offset.Z);
			result.M12 = m12;
			result.M13 = m31;
			result.M14 = 0.0f;

			result.M21 = m12;
			result.M22 = matrix.M22 + mass * (offset.Z * offset.Z + offset.X * offset.X);
			result.M23 = m23;
			result.M24 = 0.0f;

			result.M31 = m31;
			result.M32 = m23;
			result.M33 = matrix.M33 + mass * (offset.X * offset.X + offset.Y * offset.Y);
			result.M34 = 0.0f;

			result.M41 = 0.0f;
			result.M42 = 0.0f;
			result.M43 = 0.0f;
			result.M44 = 1.0f;
		}

		/// <summary>
		/// Builds a view matrix from specified position and orientation.
		/// </summary>
		/// <param name="position">A <see cref="Microsoft.Xna.Framework.Vector3"/> representing the position.</param>
		/// <param name="orientation">A <see cref="Microsoft.Xna.Framework.Matrix"/> representing the orientation.</param>
		/// <returns>The view matrix.</returns>
		public static Matrix CreateView(Vector3 position, Matrix orientation)
		{
#if INLINE
#if XBOX
			Matrix result = MatrixExtension.Zero;
#else
			Matrix result;
#endif
			result.M11 = orientation.M11;
			result.M12 = orientation.M21;
			result.M13 = orientation.M31;
			result.M14 = 0.0f;

			result.M21 = orientation.M12;
			result.M22 = orientation.M22;
			result.M23 = orientation.M32;
			result.M24 = 0.0f;

			result.M31 = orientation.M13;
			result.M32 = orientation.M23;
			result.M33 = orientation.M33;
			result.M34 = 0.0f;

			result.M41 = -(orientation.M11 * position.X + orientation.M12 * position.Y + orientation.M13 * position.Z);
			result.M42 = -(orientation.M21 * position.X + orientation.M22 * position.Y + orientation.M23 * position.Z);
			result.M43 = -(orientation.M31 * position.X + orientation.M32 * position.Y + orientation.M33 * position.Z);
			result.M44 = 1.0f;

#elif REFERENCE
			Matrix result;
			MatrixExtension.Transpose33(ref orientation, out result);

			Vector3 temp;
			MatrixExtension.GetAxis(ref orientation, MatrixAxis.X, out temp);
			result.M41 = -Vector3Extension.Dot(ref temp, ref position);
			MatrixExtension.GetAxis(ref orientation, MatrixAxis.Y, out temp);
			result.M42 = -Vector3Extension.Dot(ref temp, ref position);
			MatrixExtension.GetAxis(ref orientation, MatrixAxis.Z, out temp);
			result.M43 = -Vector3Extension.Dot(ref temp, ref position);
#else
			Matrix result = MatrixExtension.Transpose33(orientation);

			result.M41 = -Vector3.Dot(orientation.GetAxis(MatrixAxis.X), position);
			result.M42 = -Vector3.Dot(orientation.GetAxis(MatrixAxis.Y), position);
			result.M43 = -Vector3.Dot(orientation.GetAxis(MatrixAxis.Z), position);
#endif
			return result;

		}

		/// <summary>
		/// Builds a view matrix from specified position and orientation.
		/// </summary>
		/// <param name="position">A <see cref="Microsoft.Xna.Framework.Vector3"/> representing the position.</param>
		/// <param name="orientation">A <see cref="Microsoft.Xna.Framework.Matrix"/> representing the orientation.</param>
		/// <param name="result">The view matrix.</param>
		public static void CreateView(ref Vector3 position, ref Matrix orientation, out Matrix result)
		{
#if INLINE
#if XBOX
			result = MatrixExtension.Zero;
#endif
			result.M11 = orientation.M11;
			result.M12 = orientation.M21;
			result.M13 = orientation.M31;
			result.M14 = 0.0f;

			result.M21 = orientation.M12;
			result.M22 = orientation.M22;
			result.M23 = orientation.M32;
			result.M24 = 0.0f;

			result.M31 = orientation.M13;
			result.M32 = orientation.M23;
			result.M33 = orientation.M33;
			result.M34 = 0.0f;

			result.M41 = -(orientation.M11 * position.X + orientation.M12 * position.Y + orientation.M13 * position.Z);
			result.M42 = -(orientation.M21 * position.X + orientation.M22 * position.Y + orientation.M23 * position.Z);
			result.M43 = -(orientation.M31 * position.X + orientation.M32 * position.Y + orientation.M33 * position.Z);
			result.M44 = 1.0f;

#elif REFERENCE
			MatrixExtension.Transpose33(ref orientation, out result);

			Vector3 temp;
			MatrixExtension.GetAxis(ref orientation, MatrixAxis.X, out temp);
			result.M41 = -Vector3Extension.Dot(ref temp, ref position);
			MatrixExtension.GetAxis(ref orientation, MatrixAxis.Y, out temp);
			result.M42 = -Vector3Extension.Dot(ref temp, ref position);
			MatrixExtension.GetAxis(ref orientation, MatrixAxis.Z, out temp);
			result.M43 = -Vector3Extension.Dot(ref temp, ref position);
#else
			result = MatrixExtension.Transpose33(orientation);

			result.M41 = -Vector3.Dot(orientation.GetAxis(MatrixAxis.X), position);
			result.M42 = -Vector3.Dot(orientation.GetAxis(MatrixAxis.Y), position);
			result.M43 = -Vector3.Dot(orientation.GetAxis(MatrixAxis.Z), position);
#endif
		}

		/// <summary>
		/// Extracts the yaw, pitch, and roll from a <see cref="Microsoft.Xna.Framework.Matrix"/> representing a rotation matrix..
		/// </summary>
		/// <param name="matrix">Source <see cref="Microsoft.Xna.Framework.Matrix"/>.</param>
		/// <param name="yaw">The yaw, expressed in radians.</param>
		/// <param name="pitch">The pitch, expressed in radians.</param>
		/// <param name="roll">The roll, expressed in radians.</param>
		public static void Decompose(this Matrix matrix, out float yaw, out float pitch, out float roll)
		{
			// Singularity at north pole.
			if (matrix.M12 > 0.998f)
			{
				yaw = (float)Math.Atan2(matrix.M31, matrix.M33);
				pitch = 0.0f;
				roll = MathHelper.PiOver2;

				return;
			}

			// Singularity at south pole.
			if (matrix.M12 < -0.998f)
			{
				yaw = (float)Math.Atan2(matrix.M31, matrix.M33);
				pitch = 0.0f;
				roll = -MathHelper.PiOver2;

				return;
			}

			yaw = (float)Math.Atan2(-matrix.M13, matrix.M11);
			pitch = (float)Math.Atan2(-matrix.M32, matrix.M22);
			roll = (float)Math.Asin(matrix.M12);
		}

		/// <summary>
		/// Extracts the yaw, pitch, and roll from a <see cref="Microsoft.Xna.Framework.Matrix"/> representing a rotation matrix..
		/// </summary>
		/// <param name="matrix">Source <see cref="Microsoft.Xna.Framework.Matrix"/>.</param>
		/// <param name="yaw">The yaw, expressed in radians.</param>
		/// <param name="pitch">The pitch, expressed in radians.</param>
		/// <param name="roll">The roll, expressed in radians.</param>
		public static void Decompose(ref Matrix matrix, out float yaw, out float pitch, out float roll)
		{
			// Singularity at north pole.
			if (matrix.M12 > 0.998f)
			{
				yaw = (float)Math.Atan2(matrix.M31, matrix.M33);
				pitch = 0.0f;
				roll = MathHelper.PiOver2;

				return;
			}

			// Singularity at south pole.
			if (matrix.M12 < -0.998f)
			{
				yaw = (float)Math.Atan2(matrix.M31, matrix.M33);
				pitch = 0.0f;
				roll = -MathHelper.PiOver2;

				return;
			}

			yaw = (float)Math.Atan2(-matrix.M13, matrix.M11);
			pitch = (float)Math.Atan2(-matrix.M32, matrix.M22);
			roll = (float)Math.Asin(matrix.M12);
		}

		/// <summary>
		/// Orthonormalizes the specified <see cref="Matrix"/>.
		/// </summary>
		/// <param name="matrix">The source matrix.</param>
		public static void Orthonormalize(ref Matrix matrix)
		{
			// Store matrix values.
			float m11 = matrix.M11; 
			float m12 = matrix.M12; 
			float m13 = matrix.M13;

			float m21 = matrix.M21; 
			float m22 = matrix.M22; 
			float m23 = matrix.M23;

			float m31 = matrix.M31; 
			float m32 = matrix.M32; 
			float m33 = matrix.M33;

			// x
			float lengthSquared0 = m11 * m11 + m12 * m12 + m13 * m13;
			float inverseLengthSquared0 = 1.0f / lengthSquared0;
#if FAST_INVSQRT
			float inverseLength0 = MathExtension.InvSqrt(lengthSquared0);
#else
			float inverseLength0 = 1.0f / (float)Math.Sqrt(lengthSquared0);
#endif
			m11 = m11 * inverseLength0;
			m12 = m12 * inverseLength0;
			m13 = m13 * inverseLength0;

			// y
			float dot0 = m11 * m21 + m12 * m22 + m13 * m23;
			m21 = m21 - dot0 * m11 * inverseLengthSquared0;
			m22 = m22 - dot0 * m12 * inverseLengthSquared0;
			m23 = m23 - dot0 * m13 * inverseLengthSquared0;

			float lengthSquared1 = m21 * m21 + m22 * m22 + m23 * m23;
			float inverseLengthSquared1 = 1.0f / lengthSquared1;
#if FAST_INVSQRT
			float inverseLength1 = MathExtension.InvSqrt(lengthSquared1);
#else
			float inverseLength1 = 1.0f / (float)Math.Sqrt(lengthSquared1);
#endif
			m21 = m21 * inverseLength1;
			m22 = m22 * inverseLength1;
			m23 = m23 * inverseLength1;

			// z
			dot0 = m11 * m31 + m12 * m32 + m13 * m33;
			float dot1 = m21 * m31 + m22 * m32 + m23 * m33;
			m31 = m31 - dot0 * m11 * inverseLengthSquared0 - dot1 * m21 * inverseLengthSquared1;
			m32 = m32 - dot0 * m12 * inverseLengthSquared0 - dot1 * m22 * inverseLengthSquared1;
			m33 = m33 - dot0 * m13 * inverseLengthSquared0 - dot1 * m23 * inverseLengthSquared1;

			float lengthSquared2 = m31 * m31 + m32 * m32 + m33 * m33;
#if FAST_INVSQRT
			float inverseLength2 = MathExtension.InvSqrt(lengthSquared2);
#else
			float inverseLength2 = 1.0f / (float)Math.Sqrt(lengthSquared2);
#endif
			m31 = m31 * inverseLength2;
			m32 = m32 * inverseLength2;
			m33 = m33 * inverseLength2;

			// Restore matrix values.
			matrix.M11 = m11; 
			matrix.M12 = m12; 
			matrix.M13 = m13;

			matrix.M21 = m21; 
			matrix.M22 = m22; 
			matrix.M23 = m23;

			matrix.M31 = m31; 
			matrix.M32 = m32; 
			matrix.M33 = m33;
		}
	}
}
